/**************************************************************************
** Qt Creator license header template
**   Special keywords: Programmer 2012/6/4 2012
**   Environment variables: 
**   To protect a percent sign, use '%'.
**************************************************************************/

#ifndef MODELALIGNMENT_H
#define MODELALIGNMENT_H
#include <cmath>
#include <vcg/complex/algorithms/inertia.h>
#include <vcg/math/old_lin_algebra.h>
#include <wrap/callback.h>
#include <vcg/complex/algorithms/update/bounding.h>

template<class MeshType>
class ModelAlignment
{
public:
	typedef typename MeshType::VertexType            VertexType;
	typedef typename MeshType::VertexPointer         VertexPointer;
	typedef typename MeshType::VertexIterator        VertexIterator;
	typedef typename MeshType::ConstVertexIterator   ConstVertexIterator;
	typedef typename MeshType::EdgeIterator          EdgeIterator;
	typedef typename MeshType::EdgePointer           EdgePointer;
	typedef	typename MeshType::ScalarType		     ScalarType;
	typedef typename MeshType::FaceType              FaceType;
	typedef typename MeshType::FacePointer           FacePointer;
	typedef typename MeshType::FaceIterator          FaceIterator;
	typedef typename MeshType::ConstFaceIterator     ConstFaceIterator;
	typedef typename MeshType::FaceContainer         FaceContainer;
	typedef typename MeshType::CoordType             CoordType;

	ModelAlignment(){}

	static void AlignCai(MeshType& mesh)
	{
		vcg::Point3<ScalarType> barycenter;
		vcg::Matrix33<ScalarType> coMatrix;

		//vcg::tri::Inertia<MeshType>::Covariance(mesh,barycenter,coMatrix);//not precise
		Covariance(mesh,barycenter,coMatrix);
		mesh.Tr.SetIdentity();
		mesh.Tr.SetTranslate(barycenter*-1.0f);


		//rotation
		vcg::Point3<ScalarType> eigenValues;
		vcg::Matrix33<ScalarType> eigenVectors;
		int n;

		vcg::Jacobi(coMatrix,eigenValues,eigenVectors,n);
		vcg::SortEigenvaluesAndEigenvectors(eigenValues,eigenVectors);//descending order;

		vcg::Matrix44<ScalarType> mat;
		mat.SetIdentity();
		for(int i=0;i<3;i++)
		{
			mat.SetColumn(i,eigenVectors.GetRow(i));
		}
		mesh.Tr = mat * mesh.Tr;

		//ScaleInvianceCai(mesh);
	}

	static void Align(MeshType& mesh,vcg::CallBackPos* cb)
	{

		vcg::Point3<ScalarType> barycenter;
		vcg::Matrix33<ScalarType> coMatrix;

		//vcg::tri::Inertia<MeshType>::Covariance(mesh,barycenter,coMatrix);//not precise
		Covariance(mesh,barycenter,coMatrix);
		mesh.Tr.SetIdentity();
		mesh.Tr.SetTranslate(barycenter*-1.0f);


		//rotation
		vcg::Point3<ScalarType> eigenValues;
		vcg::Matrix33<ScalarType> eigenVectors;
		int n;

		vcg::Jacobi(coMatrix,eigenValues,eigenVectors,n);
		vcg::SortEigenvaluesAndEigenvectors(eigenValues,eigenVectors);//descending order;
		
		//Add by lxj on 2016-1-21
		//mesh.dPCARatio = eigenValues.X() / eigenValues.Z();
		mesh.dPCAValue[ 0 ] = eigenValues.X();
		mesh.dPCAValue[ 1 ] = eigenValues.Y();
		mesh.dPCAValue[ 2 ] = eigenValues.Z();

		vcg::Matrix44<ScalarType> mat;
		mat.SetIdentity();
		for(int i=0;i<3;i++)
			mat.SetColumn(i,eigenVectors.GetRow(i));
		
		mesh.Tr = mat * mesh.Tr;

		//reflection
		ReflecionInviance(mesh);


		//scale
		ScaleInviance(mesh);
	}
private:

	static void Covariance(MeshType& mesh,vcg::Point3<ScalarType>& bary,vcg::Matrix33<ScalarType>& coMatrix)
	{
		// find the barycenter

		ConstFaceIterator fi;
		ScalarType area = 0.0;
		bary.SetZero();
		for(fi = mesh.face.begin(); fi != mesh.face.end(); ++fi)
			if(!(*fi).IsD())
			{
				bary += vcg::Barycenter( *fi )* vcg::DoubleArea(*fi);
				area+=vcg::DoubleArea(*fi);
			}
			bary/=area;


			coMatrix.SetZero();
			vcg::Matrix33<ScalarType> tmpMat;
			CoordType tmpCoord;
			for(VertexIterator vi = mesh.vert.begin(); vi!=mesh.vert.end();vi++)
			{
				tmpCoord = vi->P() - bary;
				tmpMat.OuterProduct(tmpCoord,tmpCoord);
				coMatrix += tmpMat;
			}

			coMatrix /= area;

	}

	//the reflection-transform matrix
	static void ReflecionInviance(MeshType& mesh)
	{

		vcg::Matrix44<ScalarType> mat;
		mat.SetIdentity();
		VertexIterator vi;
		ScalarType fx = 0.0;
		ScalarType fy = 0.0;
		ScalarType fz = 0.0;
		CoordType tmpCoord;
		for(vi = mesh.vert.begin(); vi != mesh.vert.end(); ++vi)
		{
			tmpCoord = mesh.Tr * (*vi).P();
			fx += (tmpCoord.X()>0?1.0:-1.0) * tmpCoord.X()*tmpCoord.X();
			fy += (tmpCoord.Y()>0?1.0:-1.0) * tmpCoord.Y()*tmpCoord.Y();
			fz += (tmpCoord.Z()>0?1.0:-1.0) * tmpCoord.Z()*tmpCoord.Z();
		}
		fx = fx > 0.0 ? 1.0 : -1.0;
		fy = fy > 0.0 ? 1.0 : -1.0;
		fz = fz > 0.0 ? 1.0 : -1.0;
		mat.ElementAt(0,0) = fx;
		mat.ElementAt(1,1) = fy;
		mat.ElementAt(2,2) = fz;
		mesh.Tr = mat * mesh.Tr;
	}

	static void ScaleInviance(MeshType& mesh)
	{
		vcg::Matrix44<ScalarType> mat;
		mat.SetIdentity();
		VertexIterator vi;
		ScalarType sx = 0.0;
		ScalarType sy = 0.0;
		ScalarType sz = 0.0;
		CoordType tmpCoord;
		for(vi = mesh.vert.begin(); vi != mesh.vert.end(); ++vi)
		{
			tmpCoord = mesh.Tr * (*vi).P();
			sx += std::abs(tmpCoord.X());
			sy += std::abs(tmpCoord.Y());
			sz += std::abs(tmpCoord.Z());
		}

		sx /= mesh.vert.size();
		sy /= mesh.vert.size();
		sz /= mesh.vert.size();
		ScalarType scalar = 1 / std::sqrt((sx*sx + sy*sy + sz*sz) / 3);
		mat.SetScale(scalar,scalar,scalar);

		mesh.Tr = mat * mesh.Tr;
	}
};


#endif // MODELALIGNMENT_H
